
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <ros/time.h>
#include "motor_driver/controlcan.h"
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
// #include "dirve_ros-master/dirve_ctr.h"
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tf.h>
#include "sensor_msgs/JointState.h"
#include "motor_driver/Feedback.h"
#include "motor_driver/wheel_state.h"


#define MAX_LINEAR  1.57        //m/s
#define MAX_ANGULAR  0.7853     //red/s


enum _enum_DirveID{

	DIRVEE_ID_SYSTEM_STATE_ACK = 0x151,
	DIRVEE_ID_MOVE_CONTROL_ACK = 0x131,
	DIRVEE_ID_MOVE_CONTROL = 0x130,
	DIRVEE_ID_ONE_MOTOR_ACK = 0x201,
	DIRVEE_ID_TWO_MOTOR_ACK = 0x202,
	DIRVEE_ID_THREE_MOTOR_ACK = 0x203,
	DIRVEE_ID_FOUR_MOTOR_ACK = 0x204,
	DIRVEE_ID_LAMPLIGHT_CONTROL = 0x140,		//lamplight
	DIRVEE_ID_LAMPLIGHT_CONTROL_ACK = 0x141,

   DIRVEE_ID_ONE_POSE_ACK = 0X301,
   DIRVEE_ID_TWO_POSE_ACK = 0X302,
   DIRVEE_ID_THREE_POSE_ACK = 0X303,
   DIRVEE_ID_FOUR_POSE_ACK = 0X304
 
};

struct  DIRVEDATA
{
   uint8_t carBodyStateAck;
   int8_t  modeControlAck;
   uint16_t batteryVoltageAck;
   uint16_t faultMessageAck;

   int16_t movementSpeedAck;
   int16_t rotationalSpeedAck;
	
   uint8_t controlMode;
   uint8_t faultClearance;
   int8_t  percentLinear;
   int8_t  percentAngular;
   
   uint16_t oneMotorElectricCurrentAck;
   int16_t oneMotorRorateSpeedAck;
   int8_t  oneMotorTemperatureAck;
   
   uint16_t twoMotorElectricCurrentAck;
   int16_t twoMotorRorateSpeedAck;
   int8_t  twoMotorTemperatureAck;

   uint16_t threeMotorElectricCurrentAck;
   int16_t threeMotorRorateSpeedAck;
   int8_t  threeMotorTemperatureAck;

   uint16_t fourMotorElectricCurrentAck;
   int16_t fourMotorRorateSpeedAck;
   int8_t  fourMotorTemperatureAck;

   uint8_t lightControlEnableSign;
   uint8_t bodyLightPattern;

   uint8_t currentLightControlEnableSignAck;
   uint8_t bodyLightPatternAck;
} ;

class DriverPub
{
   public:
        DriverPub();
        ~DriverPub();
        void loop();

    private:
        bool initRobot();
        void receive_func();
        void send_speed_callback(void);
        void handle_speed_data(void);
        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
        uint8_t get_line_speed(void);
        uint8_t get_angular_speed(void);
        void setSendData_speed(uint8_t line_speed,uint8_t angular_speed);
        bool check_sum(VCI_CAN_OBJ * rec_buff);
        void dirve_ProcessData(VCI_CAN_OBJ *p,DIRVEDATA *PdirveDate );
        void Calculate_LeftAndRight_Meter(VCI_CAN_OBJ *p);
        uint8_t get_recvFlag(void);
        int32_t get_rev_leftVol(void);
        int32_t get_rev_rightVol(void);
        void set_walkingmotor_speed_angle(double line_speed,double angle_speed);

        boost::mutex cmd_vel_mutex_;
        ros::Time last_twist_time_;
        
        boost::mutex mutex_;

        ros::Subscriber cmd_sub_;
        ros::Publisher odom_pub_;
        ros::Publisher wheel_state_feedback_;
        motor_driver::Feedback  left_right_wheel_state_;
        
      //   ros::Publisher front_right_wheel_state_;
      //   os::Publisher rear_left_wheel_state_;
      //   ros::Publisher rear_right_wheel_state_;

        std::string odom_frame_;
        std::string base_frame_;

        bool recv_flag_;
        bool start_flag_;
        bool publish_odom_transform_;
        geometry_msgs::Twist current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
         tf2_ros::TransformBroadcaster br_;
        

        int ret;
        // pthread_t threadid;
        ros::Time now_;
        ros::Time last_time_;
        VCI_BOARD_INFO pInfo;//用来获取设备信息。
        VCI_BOARD_INFO pInfo1 [50];
        // short error;

        double wheel_track_4wd_;
        double encoder_resolution_4wd_;
        double ticks_per_meter_;       //脉冲数/m

        double linear_correction_factor_;
        double angular_correction_factor_;
        double gear_reduction_4wd_;     //减速比
        double wheel_diameter_4wd_;
        double start_rotation_limit_w_;
        double delta_time_;
        double delta_xy_ave_;
        double delta_th_;
        double v_linear_;
        double v_angular_;
        double delta_x_;
        double delta_y_;

        double accumulation_x_;
        double accumulation_y_;
        double accumulation_th_;
        double accumulation_left_;
        double accumulation_left_1_;
        double accumulation_right_;
        double accumulation_right_1_;

        double delta_left_;
        double delta_right_;        //里程差值 单位 m
        double delta_left_1_;
        double delta_right_1_;        //里程差值 单位 m
        int32_t rev_left_;
        int32_t rev_left_1_;
        int32_t rev_right_;       
        int32_t rev_right_1_;
        int cur_left_;              //存放脉冲计数值
        int cur_left_1_; 
        int cur_right_;
        int cur_right_1_;
        double encoder_low_wrap_;       //解决编码器溢出
        double encoder_high_wrap_;
        int l_wheel_mult_;              //多圈计数
        int l_wheel_mult_1_;              //多圈计数
        int r_wheel_mult_;
        int r_wheel_mult_1_;
        int encoder_min_;
        int encoder_max_;   

        int32_t line_speed_percent;
        int32_t angle_speed_percent;

      //   int32_t rev_left_;
      //   int32_t rev_right_;
        uint8_t queueFlag;
        uint8_t can_recv_flag;

};


